import RPi.GPIO as GPIO
#超声波避障
from .Adafruit_PWM_Servo_Driver import PWM
import time

servoMin = 150  # Min pulse length out of 4096
servoMax = 600  # Max pulse length out of 4096
SLEEP = 0.5

class Adafruit(object):
    def __init__(self,address=0x40,trig=20,echo=21, debug=False):
        # Initialise the PWM device using the default address
        # bmp = PWM(0x40, debug=True)
        self.pwm = PWM(address,debug)
        self.TRIG = trig
        self.ECHO = echo
        
    #初始化
    def init(self):
        GPIO.setup(self.TRIG, GPIO.OUT)
        GPIO.setup(self.ECHO, GPIO.IN)
        self.pwm.setPWMFreq(50)                        # Set frequency to 60 Hz
        
    #启动
    def start(self):        
        pass
    
    def setServoPulse(self,channel, pulse):
      pulseLength = 1000000.0                   # 1,000,000 us per second
      pulseLength /= 50.0                       # 60 Hz
      # print("%d us per period" % pulseLength)
      pulseLength /= 4096.0                     # 12 bits of resolution
      # print("%d us per bit" % pulseLength)
      pulse *= 1000.0
      pulse /= (pulseLength*1.0)
    # pwmV=int(pluse)
      # print("pluse: %f  " % (pulse))
      self.pwm.setPWM(channel, 0, int(pulse))

    #Angle to PWM
    def write(self,servonum,x):
      y=x/90.0+0.5
      y=max(y,0.5)
      y=min(y,2.5)
      self.setServoPulse(servonum,y)

    def distance(self):
        GPIO.output(self.TRIG, 0)
        time.sleep(0.000002)

        GPIO.output(self.TRIG, 1)
        time.sleep(0.00001)
        GPIO.output(self.TRIG, 0)


        while GPIO.input(self.ECHO) == 0:
            a = 0
        time1 = time.time()
        while GPIO.input(self.ECHO) == 1:
            a = 1
        time2 = time.time()

        during = time2 - time1
        return during * 340 / 2 * 100
    
    #根据180角度的百分比角度来采集距离，is_opposite是否反向
    def angel_dist(self,rate,is_opposite=False):
        val = int(170*rate)
        if is_opposite == True:
            val = 170 - val
        val += 5
        
        self.write(0,val)
        time.sleep(SLEEP)
        return self.distance()
        

    def front(self):
        self.write(0,90)
        time.sleep(SLEEP)
        return self.distance()
    
    def left_rate(self,rate):
        val = int((175-90)*rate+90)
        self.write(0,val)
        time.sleep(SLEEP)
        return self.distance()
    
    def right_rate(self,rate):
        val = int(90-(90-5)*rate)
        self.write(0,val)
        time.sleep(SLEEP)
        return self.distance()
        
    def left_front(self):
         self.write(0, 135)
         time.sleep(SLEEP)
         return self.distance()

    def left(self):
         self.write(0, 175)
         time.sleep(SLEEP)
         return self.distance()
            
    def right_front(self):
        self.write(0,45)
        time.sleep(SLEEP)
        return self.distance()

    def right(self):
        self.write(0,5)
        time.sleep(SLEEP)
        return self.distance()